/*
spiCAN.cpp - Library for talking to CAN controller
 */

#include "WProgram.h"
#include "spiCAN.h"
#include "can_constants.h"

spiCAN::spiCAN(int CSPin,int RTS0Pin,int RTS1Pin,int RTS2Pin){//constructor
  // initialize the SPI pins
  pinMode(SCK_PIN, OUTPUT);
  pinMode(MOSI_PIN, OUTPUT);
  pinMode(MISO_PIN, INPUT);
  csPin = CSPin;
  rts0Pin=RTS0Pin;
  rts1Pin=RTS1Pin;
  rts2Pin=RTS2Pin;
  pinMode(csPin,OUTPUT);
  digitalWrite(csPin,HIGH);
  transmitstate=false;
  transmitpins=false; //sets whether pins or spi commands initiate transmission

  // enable SPI Master, MSB, SPI mode 0, FOSC/4
  mode(0);
}

void spiCAN::initController(){
  if(transmitpins){
  digitalWrite(rts0Pin,HIGH); //set rts pins high so we don't shootoff messages
  pinMode(rts0Pin,OUTPUT);
  digitalWrite(rts1Pin,HIGH);
  pinMode(rts1Pin,OUTPUT);
  digitalWrite(rts2Pin,HIGH);
  pinMode(rts2Pin,OUTPUT);
  }
  byte cnf_settings[] = {
  CNF3_VAL,CNF2_VAL,CNF1_VAL};
  transfer(RESET); 
  writeRegister(CNF3,cnf_settings,3); //configure timing settins by starting at CNF3
  if(transmitpins){
  writeRegister(TXRTSCTRL,0b00000111);  //enable RTSTXx Pins
  }
  bitModRegister(RXB0CTRL,0b01100000,0b00100000);   //setup RX buffers and int pins
  bitModRegister(RXB1CTRL,0b01100000,0b00100000);  
  bitModRegister(BFPCTRL,0b00001111,0b00001111);
  beginTrans(RXM0SIDH,0);  //load SIDH/L mask as zero
  transfer(0);
  endTrans();
  beginTrans(RXM1SIDH,0);  //load SIDH/L mask as zero
  transfer(0);
  endTrans();
  writeRegister(CANINTE,0b00000011); //enable INT# pin forthe two RXbufs
  writeRegister(CANCTRL,0b00000011);

}
  
void spiCAN::beginTrans(byte instr, byte data){
  transmitstate = true; //set transmit state to true
  togglePinLow(csPin);  //select the slave
  spitransfer(instr);  //send instr byte
  spitransfer(data);
}
void spiCAN::transfer(byte data){
  if (!transmitstate){   /*if a new transmission hasn't already started, set CS line low to indicate new transmission. This is necessary because some SPI t
   ansfer only consist of an intruction/address byte with no data bytes following*/
    togglePinLow(csPin);
    spitransfer(data);
    endTrans();//if the method was called before a transmission had officially began then we'll end the transmission right here
  }
  else {
    spitransfer(data);
  }

}
byte spiCAN::readbyte(){ //this function won't do anything unless a cs has already been pulled low
  return spitransfer(0xFF); //when we want to read an SPI byte, 0xFF is shifted out. This actually sends no data over MOSI but keeps SCK toggling.
}
void spiCAN::endTrans(){
  digitalWrite(csPin,HIGH); //set CS high and set transmit state to false;
  transmitstate = false;
} 
void spiCAN::writeRegister(byte address,byte data[], byte numbytes){
  beginTrans(WRITE,address);
  for (byte i=0;i<numbytes;i++){
    transfer(data[i]);
  }
  endTrans();
}
void spiCAN::writeRegister(byte address,byte data){
  beginTrans(WRITE,address);
  transfer(data);
  endTrans();
}
byte spiCAN::readRegister(byte address){ //this closes SPI transmission
  byte dummy = readRegisterCont(address);
  endTrans();
  return dummy;
}
byte spiCAN::readRegisterCont(byte address){ //this function keeps SPI open
  beginTrans(READ,address);
  return readbyte();
}
void spiCAN::bitModRegister(byte address,byte mask,byte data){
  beginTrans(BIT_MODIFY,address);
  transfer(mask);
  transfer(data);
  endTrans();
}

Datapack spiCAN::readRXBuf(boolean bufnum){
   Datapack a; 
   byte rxsidh;
    byte rxdlc;
    byte intbit;  

    switch(bufnum){
      case false:
      rxsidh = RXB0SIDH;
      rxdlc = RXB0DLC;
      intbit = 1;
      break;
      case true:
      rxsidh = RXB1SIDH;
      rxdlc = RXB1DLC;
      intbit = 2;
      break;
    }
    
    a.sid=readRegisterCont(rxsidh)<<3;
    a.sid=a.sid|readbyte()>>5;
    endTrans();
    a.dlc = readRegisterCont(rxdlc)&0b00001111;  //#of bytes in can message

    for (byte i=0;i<a.dlc&&i<8;i++){
      a.data[i]=readbyte();

    }
    endTrans();
    bitModRegister(CANINTF,intbit,0); //clear interrupt flag on that buffer bit

   return a;
}

void spiCAN::mode(byte config){
  byte tmp;
  // enable SPI master with configuration byte specified
  SPCR = 0;
  SPCR = (config & 0x7F) | (1<<SPE) | (1<<MSTR);
  tmp = SPSR;
  tmp = SPDR;
}
byte spiCAN::spitransfer(byte value){
  SPDR = value;
  while (!(SPSR & (1<<SPIF))) ;
  return SPDR;
}
void spiCAN::togglePinLow(int Pin){
  digitalWrite(Pin,HIGH);
  digitalWrite(Pin,LOW);
}
void spiCAN::togglePinHigh(int Pin){
  digitalWrite(Pin,LOW);
  digitalWrite(Pin,HIGH);
}

byte spiCAN::freeTXBuffer(){ //this function returns a TX buffer with no pending transmission. The byte returned corresponds to SIDH address 
  while (1){    //we keep looping until we find a free TX buffer
    if(bitRead(readRegister(TXB0CTRL),3)==0){
      return 0x31; 
    }
    if (bitRead(readRegister(TXB1CTRL),3)==0){
      return 0x41;
    }
    if (bitRead(readRegister(TXB2CTRL),3)==0){
      return 0x51; 
    }
  }
}

void spiCAN::sendTXBuffer(byte adr){ //initiates buffer send request depending on the address passed
  switch(adr){
  case 0x31:
  if(transmitpins){
    togglePinHigh(rts0Pin);}
    else{
    //Serial.print(rts0Pin,DEC);
    transfer(RTS_TX0);}
    break;
  case 0x41:
  if(transmitpins){
    togglePinHigh(rts1Pin);}
    //    Serial.print(rts1Pin,DEC);
    else{transfer(RTS_TX1);}
    break;
  case 0x51:
    if(transmitpins){togglePinHigh(rts2Pin);}
  //      Serial.print(rts2Pin,DEC);
    else{transfer(RTS_TX2);}
    break;
  }
}


void spiCAN::sendCANMessage(int sid,byte dlc,byte data[]){
  byte adr = freeTXBuffer();
 
  writeRegister(adr,sid>>3);
  writeRegister(adr+1,lowByte(sid)<<5);
  writeRegister(adr+4,dlc); //load DLC into DLC register (data length code)
  writeRegister(adr+5,data,dlc); //place bytes into registers
  sendTXBuffer(adr);
  }
  
  void spiCAN::sendCANMessage(Datapack a){

  byte adr = freeTXBuffer();
  writeRegister(adr,a.sid>>3);
  writeRegister(adr+1,lowByte(a.sid)<<5);
  writeRegister(adr+4,a.dlc); //DLC is 4 bytes after SIDH, 
  writeRegister(adr+5,a.data,a.dlc); //DLC is 4 bytes after SIDH, 
  sendTXBuffer(adr);
  }
  

